#include "C:\Users\InDenProCom\Google Drive\2014-2015 stuff\Code\HTSPB I2C Code\htspb-i2c-master\i2c\i2c.h"

#define Addr 0x6B
const tSensors HTSPB = msensor_S2_4;

int x, y, xlo, xhi, ylo, yhi;
float scaleX, scaleY;

void i2cInit();
void updateVals();
float getX();
float getY();


void i2cInit()
{
	i2c_begin(HTSPB, 0, 1); //initialize I2C library
	i2c_beginTransmission(Addr);
	i2c_write(0x0F);
	i2c_endTransmission();

	i2c_requestFrom(Addr, (byte)1);//Request byte from accel to confirm we're connected
	byte value = i2c_read();
	i2c_endTransmission();
	//while(true){
	//	if (value == 0xD7)
	//	{
	//	nxtDisplayCenteredTextLine(1, "True");
	//	}
	//	else
	//	{
	//		nxtDisplayCenteredTextLine(1, "False: %x", value);
	//	}

	////write 2 bytes to set continuous mode
	i2c_beginTransmission(Addr);
	i2c_write((byte)0x20);
	i2c_write(0x0F);
	i2c_endTransmission();

	i2c_beginTransmission(Addr);//Set accel scaling
	i2c_write((byte)0x23);
	i2c_write(0x20);
	i2c_endTransmission();

}


void updateVals()
{
		i2c_beginTransmission(Addr);
		i2c_write(0x28 | 0x80);//Set auto-increment bit
		i2c_endTransmission();
		i2c_requestFrom(Addr, (byte)6);//We have to get all 6 bytes(?) or else it won't work
		while (i2c_available() < 6)
		{
		}
		xlo = i2c_read();//Get x high and low bytes
		xhi = i2c_read();
		x = (xlo | (xhi << 8));

		ylo = i2c_read();//Get y high and low bytes
		yhi = i2c_read();
		y = (ylo | (yhi << 8));
		i2c_read();//Get 2 extra bytes for Z axis; we don't care about them.
		i2c_read();

		if (abs(x)<15)//Set value scaling
		{
			scaleX = 0;
		}
		else
		{
			scaleX = x/15;
		}
		if (abs(y)<15)
		{
			scaleY = 0;
		}
		else
		{
			scaleY = y/15;
		}
}

float getX()
{
	return scaleX;
}

float getY()
{
	return scaleY;
}
